#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <pthread.h>
#include <semaphore.h>
#include <errno.h>

#include "Run_queue.h"
#include "new.h"

#define ERR_INVAILID_ARGS -1

#define INIT_LOCK(pLock)   do {\
	pthread_mutex_t x = PTHREAD_MUTEX_INITIALIZER;\
	(*(pLock)) = x;\
} while(0)

#define QUEUE_MOVE_AHEAD(q,x) ((x)<(q->nCharCapacity))?((x)+(q->nSize)):0

#define QUEUE_MOVE_BACK(q,x) ((x)>0)?(x-(q->nSize)):(q->nCapacity)

#define LOCK(pLock) pthread_mutex_lock(pLock)
#define UNLOCK(pLock) pthread_mutex_unlock(pLock)
#define DESTORY_LOCK(pLock) pthread_mutex_destroy(pLock)
#define SEM_VALUE(pSem,pValue) do {\
	sem_getvalue(pSem, pValue); \
}while(0)
#define SEM_TRY_WAIT(pSem) sem_trywait(pSem)

struct _SEMAPHORE
{
	sem_t hInternalSem;
};

typedef struct _SEMAPHORE SEMAPHORE;

struct _QUEUE
{
	int nIncStep;
	int nSize;   //size of an queue's element
	int nCapacity;  //max elements of the queue
	char *pBuffer; //the fifo buffer
	pthread_mutex_t hSyncLock;
	int nCount;    //the current elements in the queue
	int nCharCapacity;   //equal to (nCapacity-1)*nSize in char, this variable 
						 //is use to move back and ahead Marco
	int nHead;  //the first char of the head position
	int nTail;  //the first char 0f the tail empty position

	void_p hCountSem;   //the semaphore for the count
};

typedef struct _QUEUE QUEUE;

void_p Queue_Create(int nMaxCapacity, int nElementSize, int nIncStep)
{
	if(nMaxCapacity<=0||nElementSize<=0||nIncStep<0)
		return NULL;
	
	QUEUE *q = NEW(QUEUE, 1);
	if(q==NULL)
		return NULL;

	q->pBuffer = NEW(char, nElementSize*nMaxCapacity);
	if(q->pBuffer==NULL)
	{
		DELETE(q);
		return NULL;
	}
	
	SEMAPHORE *pSem = NEW(SEMAPHORE, 1);
	if(pSem==NULL)
	{
		DELETE(q->pBuffer);
		DELETE(q);
		return NULL;
	}
	
	sem_init(&pSem->hInternalSem, 0, 1);
	
	q->hCountSem = (void_p)pSem;
	INIT_LOCK(&q->hSyncLock);

	q->nIncStep = nIncStep;
	q->nSize = nElementSize;
	q->nCapacity = nMaxCapacity;
	q->nCount = 0;
	q->nCharCapacity = (q->nCapacity-1)*q->nSize;
	q->nHead = q->nTail = 0;

	return (void_p)q;
}


int Queue_Put(void_p q, void_p pElement, BOOL bUrgent)
{
	if(q==NULL||pElement==NULL)
		return ERR_INVAILID_ARGS;

	QUEUE *pq = (QUEUE*)q;
	int nResult = 0;

	LOCK(&pq->hSyncLock);
	
	if(pq->nCount>= pq->nCapacity)
	{
		if(pq->nIncStep>0)
		{
			int nNewSize = (pq->nCapacity+pq->nIncStep)*pq->nSize;
			char * pNewBuffer = NEW(char, nNewSize);
			if(pNewBuffer!=NULL)
			{
				int offset = pq->nHead/pq->nSize;
				memcpy(pNewBuffer, pq->pBuffer+pq->nHead, (pq->nCount-offset)*pq->nSize);
				memcpy(pNewBuffer+(pq->nCount-offset)*pq->nSize, pq->pBuffer, pq->nTail);
				DELETE(pq->pBuffer);
				pq->pBuffer = pNewBuffer;
				pq->nCapacity += pq->nIncStep;
				pq->nCharCapacity = (pq->nCapacity-1)*pq->nSize;
				pq->nHead = 0;
				pq->nTail = pq->nCount*pq->nSize;
			}
			else
				nResult = -1;
		}
		else 
			nResult = -1;
	}

	if(pq->nCount<pq->nCapacity)
	{
		if(bUrgent==FALSE)
		{
			memcpy(&(pq->pBuffer[pq->nTail]), pElement, pq->nSize);
			pq->nTail = QUEUE_MOVE_AHEAD(pq, pq->nTail);
		}
		else   //emergency , put to nHead
		{
			pq->nHead = QUEUE_MOVE_BACK(pq, pq->nHead);
			memcpy(pq->pBuffer, pElement, pq->nSize);
		}

		pq->nCount++;

		sem_post(&((SEMAPHORE*)(pq->hCountSem))->hInternalSem);
	}

	UNLOCK(&pq->hSyncLock);
	return nResult;
}

int Queue_GetCount(void_p q)
{
	QUEUE *pq = (QUEUE*)q;
	return pq->nCount;
}

int Queue_Get(void_p q, void_p pElement, BOOL bPeek, DWORD dwTimeout)
{
	if(q==NULL||pElement==NULL)
		return -1;
	
	QUEUE* pq = (QUEUE*)q;
	struct timespec period;
	period.tv_sec = (time_t)(dwTimeout/1000);
	period.tv_nsec = dwTimeout%1000*1000;
	int i=0;
	while(i<3)
	{
		int waitResult = sem_timedwait(&((SEMAPHORE*)(pq->hCountSem))->hInternalSem, &period);
		if(waitResult==ETIMEDOUT)
			i++;
		else if(waitResult==0)
			break;
		else 
			return -1;
	}

	int nResult = 0;
	LOCK(&pq->hSyncLock);
	if(pq->nCount>0)
	{
		memcpy(pElement, &(pq->pBuffer[pq->nHead]), pq->nSize);
		if(bPeek==FALSE)
		{
			pq->nHead = QUEUE_MOVE_AHEAD(pq, pq->nHead);
			pq->nCount--;
		}
	}
	else
		nResult = -1;
	
	UNLOCK(&pq->hSyncLock);
	if(bPeek==TRUE)
		sem_post(&((SEMAPHORE*)(pq->hCountSem))->hInternalSem);

	return nResult;
}

void Queue_Make_Empty(void_p q)
{
	if(q==NULL)
		return;
		
	QUEUE *pq = (QUEUE*)q;
	
	LOCK(&pq->hSyncLock);
	SEMAPHORE *pSem = (SEMAPHORE*)pq->hCountSem;
	int semValue = 0;
	SEM_VALUE(&(pSem->hInternalSem), &semValue);
	while(semValue>0)
	{
		int iRes = SEM_TRY_WAIT(&(pSem->hInternalSem));
		SEM_VALUE(&(pSem->hInternalSem), &semValue);
		if(iRes==EAGAIN&&semValue>0);
			usleep(5);
		SEM_VALUE(&(pSem->hInternalSem), &semValue);
	}
	
	pq->nCount = pq->nHead = pq->nTail = 0;
	UNLOCK(&pq->hSyncLock);
}



void Queue_Destory(void_p q)
{
	if(q==NULL)
		return;

	QUEUE *pq = (QUEUE*)q;
	LOCK(&pq->hSyncLock);
	
	DELETE(pq->pBuffer);
	sem_destroy(&((SEMAPHORE*)(pq->hCountSem))->hInternalSem);
	DELETE(pq->hCountSem);
	
	UNLOCK(&pq->hSyncLock);
	DELETE(q);
}


